/*!
 * @file Simulation.h
 * @brief Main simulation class
 */

#ifndef PROJECT_SIMULATION_H
#define PROJECT_SIMULATION_H

#include "ControlParameters/ControlParameterInterface.h"
#include "ControlParameters/RobotParameters.h"
#include "ControlParameters/SimulatorParameters.h"
#include "Dynamics/Cheetah3.h"
#include "Dynamics/MiniCheetah.h"
#include "Dynamics/Quadruped.h"
#include "Graphics3D.h"
#include "SimUtilities/ImuSimulator.h"
#include "SimUtilities/SimulatorMessage.h"
#include "SimUtilities/SpineBoard.h"
#include "SimUtilities/ti_boardcontrol.h"
#include "Utilities/SharedMemory.h"
#include "Utilities/Timer.h"

#include <mutex>
#include <queue>
#include <utility>
#include <vector>

#include <lcm/lcm-cpp.hpp>
#include "simulator_lcmt.hpp"

#define SIM_LCM_NAME "simulator_state"

/*!
 * Top-level control of a simulation.
 * A simulation includes 1 robot and 1 controller
 * It does not include the graphics window: this must be set with the setWindow
 * method
 */
class Simulation {
    friend class SimControlPanel;
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    explicit Simulation(RobotType robot, Graphics3D* window,
                        SimulatorControlParameters& params, ControlParameters& userParams,
                        std::function<void(void)> ui_update);

    /*!
   * Explicitly set the state of the robot
   */
    void setRobotState(FBModelState<double>& state) {
        _simulator->setState(state);
    }

    void step(double dt, double dtLowLevelControl, double dtHighLevelControl);

    void addCollisionPlane(double mu, double resti, double height,
                           double sizeX = 20, double sizeY = 20,
                           double checkerX = 40, double checkerY = 40,
                           bool addToWindow = true);
    void addCollisionBox(double mu, double resti, double depth, double width,
                         double height, const Vec3<double>& pos,
                         const Mat3<double>& ori, bool addToWindow = true,
                         bool transparent = true);
    void addCollisionMesh(double mu, double resti, double grid_size,
                          const Vec3<double>& left_corner_loc,
                          const DMat<double>& height_map, bool addToWindow = true,
                          bool transparent = true);

    void lowLevelControl();
    void highLevelControl();

    /*!
   * Updates the graphics from the connected window
   */
    void updateGraphics();

    void runAtSpeed(std::function<void(std::string)> error_callback, bool graphics = true);
    void sendControlParameter(const std::string& name,
                              ControlParameterValue value,
                              ControlParameterValueKind kind,
                              bool isUser);

    void resetSimTime() {
        _currentSimTime = 0.;
        _timeOfNextLowLevelControl = 0.;
        _timeOfNextHighLevelControl = 0.;
    }

    ~Simulation() {
        delete _simulator;
        delete _robotDataSimulator;
        delete _imuSimulator;
        delete _lcm;
    }

    const FBModelState<double>& getRobotState() { return _simulator->getState(); }

    void stop()
    {
        _running = false;  // kill simulation loop
        _wantStop = true;  // if we're still trying to connect, this will kill us

        if (_connected) {
            _sharedMemory().simToRobot.mode = SimulatorMode::EXIT;
            _sharedMemory().simulatorIsDone();
        }
    }

    SimulatorControlParameters& getSimParams() { return _simParams; }

    RobotControlParameters& getRobotParams() { return _robotParams; }
    ControlParameters& getUserParams() { return _userParams; }

    bool isRobotConnected() { return _connected; }

    void firstRun();
    void buildLcmMessage();
    void loadTerrainFile(const std::string& terrainFileName,
                         bool addGraphics = true);

private:
    void handleControlError();
    Graphics3D* _window = nullptr;

    std::mutex _robotMutex;
    SharedMemoryObject<SimulatorSyncronizedMessage> _sharedMemory;
    ImuSimulator<double>* _imuSimulator = nullptr;
    SimulatorControlParameters& _simParams;
    ControlParameters& _userParams;
    RobotControlParameters _robotParams;

    size_t _simRobotID, _controllerRobotID;
    Quadruped<double> _quadruped;
    FBModelState<double> _robotControllerState;
    FloatingBaseModel<double> _model;
    FloatingBaseModel<double> _robotDataModel;
    DVec<double> _tau;
    DynamicsSimulator<double>* _simulator = nullptr;
    DynamicsSimulator<double>* _robotDataSimulator = nullptr;
    std::vector<ActuatorModel<double>> _actuatorModels;
    SpiCommand _spiCommand;
    SpiData _spiData;
    SpineBoard _spineBoards[4];
    TI_BoardControl _tiBoards[4];
    RobotType _robot;
    lcm::LCM* _lcm = nullptr;

    std::function<void(void)> _uiUpdate;
    std::function<void(std::string)> _errorCallback;
    bool _running = false;
    bool _connected = false;
    bool _wantStop = false;
    double _desiredSimSpeed = 1.;
    double _currentSimTime = 0.;
    double _timeOfNextLowLevelControl = 0.;
    double _timeOfNextHighLevelControl = 0.;
    s64 _highLevelIterations = 0;
    simulator_lcmt _simLCM;
};

#endif  // PROJECT_SIMULATION_H
